// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CameraPoseEstimationAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Runtime.Serialization; 

    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.GroundTruthExtraction;
    using Microsoft.Robotics.Vision.Runtime.Cameras;    
  
    /// <summary>
    /// Camera pose estimation agent which consumes a RGB Camera message and publishes a camera pose estimation agent message.
    /// This class is responsible for reporting the camera pose estimate and the corresponding landmark point. 
    /// It still reports the raw landmark translation and rotation from MSR codebase which is used by the navigation metrics. 
    /// However, when navigation metrics is updated, then it will switch to using the robot pose relative to landmark since that has tilt calibration and is more accurate. 
    /// The current landmark id is set on this class externally using a control message
    /// (Using either the UI or by the PathPlanningMetrics agent or SlamPlayback agent). 
    /// When landmark id detection is added, this control port can be removed. 
    /// The web camera calibration parameters are externalized from this agent’s manifest and moved to WebcamCalibration.xml
    /// in the same folder (ev4CalibrationFolder) used for Kinect Camera Calibration. 
    /// Obsolete: Task 2224
    /// </summary>
    [System.Diagnostics.CodeAnalysis.SuppressMessage(
    "Microsoft.Design",
    "CA1001:TypesThatOwnDisposableFieldsShouldBeDisposable",
    Justification = "PipelineElements clean up resources when OnDeactivated() is called")]
    [DataContract]
    public class CameraPoseEstimationAgent : ConsumerProducerAgent<RGBCameraAgentMessage, CameraPoseEstimationAgentMessage>, IControlReceiver<CameraPoseEstimationInputMessage>
    {
        /// <summary>
        /// Robot configuration folder which contains camera calibration information.
        /// </summary>
        [DataMember(Name = "RobotConfigFolder")]
        private readonly string robotConfigFolder;

        /// <summary>
        /// Environment configuration folder which contains environment specific information such as maps, landmarks info etc.
        /// </summary>
        [DataMember(Name = "EnvironmentConfigFolder")]
        private readonly string environmentConfigFolder;

        /// <summary>
        /// The landmark id of the current landmark. Currently this is provided as a control message from the metrics controller agent as it knows the
        /// landmark id for each waypoint. However, once we implement the landmark detection code, the camera pose estimation agent
        /// will itself determine the landmark id.
        /// </summary>
        private int? currentLandmarkId;

        /// <summary>
        /// Camera pose estimator, which converts the planar pose to landmark relative camera pose in landmark coordinates.
        /// </summary>
        private UpwardFacingCameraPoseEstimator cameraPoseEstimator;

        /// <summary>
        /// Initializes a new instance of the CameraPoseEstimationAgent class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">RGB Camera Agent Message Producer.</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="environmentConfigFolder">Environment config folder</param>
        public CameraPoseEstimationAgent(
            string name,
            IProducer<RGBCameraAgentMessage> producer,
            string robotConfigFolder,
            string environmentConfigFolder)
            : base(name, producer.Name)
        {
            this.robotConfigFolder = robotConfigFolder;
            this.environmentConfigFolder = environmentConfigFolder;
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            this.cameraPoseEstimator = new UpwardFacingCameraPoseEstimator(this.robotConfigFolder, this.environmentConfigFolder);
            base.OnActivated();
        }

        /// <summary>
        /// Called when the agent is about to end
        /// </summary>
        public override void OnDeactivated()
        {
            this.cameraPoseEstimator.Dispose();
            base.OnDeactivated();
        }

        /// <summary>
        /// Receive an image, process it and publish the camera pose, corresponding landmark point and planar checkerboard pose .
        /// </summary>
        /// <param name="message">Message to process</param>
        public override void Receive(RGBCameraAgentMessage message)
        {
            ImageFrameGray8 lumImage = ImageOperations2DInterop.ConvertYUY2ToGray8Image(message.RGBImageFrame);

            Pose2D cameraPose;
            Vector3 rotations;
            Vector3 translations;

            CameraPoseEstimationAgentMessage groundTruthMessage;

            if (this.cameraPoseEstimator.TryGetCameraPose(lumImage, out cameraPose, out rotations, out translations))
            {
                groundTruthMessage = new CameraPoseEstimationAgentMessage(
                    message.OriginatingTime,
                    new Vector3(translations.X, translations.Y, translations.Z),
                    new Vector3(rotations.X, rotations.Y, rotations.Z),
                    cameraPose,
                    this.cameraPoseEstimator.LookupLandmark(this.currentLandmarkId));
            }
            else
            {
                groundTruthMessage = new CameraPoseEstimationAgentMessage(
                message.OriginatingTime); 
            }

            this.Publisher.Post(groundTruthMessage);
        }

        /// <summary>
        /// Receives and stores the current landmark id.
        /// </summary>
        /// <param name="message">Camera pose estimation input message containing the landmark id</param>
        public void ReceiveControl(CameraPoseEstimationInputMessage message)
        {
            TraceOut.Info(TraceContexts.Vision, "CameraPoseEstimationAgent received control message: LandmarkId {0}", message.LandmarkId);
            this.currentLandmarkId = message.LandmarkId;
        }
    }
}
